/* 
相位输出范围0~2π，相位0对应正弦最大值（即0对应幅值1pu）
参考《单相电力锁相环的研究》

对外接口：对外接口：_Creat(),_Cal()

 */

#include <iostream>
#include <tuple>
#include <utility>
#include <cmath>

#include "../include/_PLL_SOGI.h"

#define π 3.141592653589793238462643383279

//静止转同步坐标系
static auto  αβ2dq( floatx α,floatx β,floatx θ )
{
    auto cost = cos(θ);
    auto sint = sin(θ);
    auto d = cost*α + sint*β;
    auto q = -sint*α + cost*β;      
    return std::make_tuple(d,q);     
}

//循环积分，输出在某一区间
static auto 积分_循环( floatx x ,floatx Ts ,floatx lower,floatx upper,floatx y_n_1)
{   
    //y_n_1:上一次输出值
    auto yt = y_n_1;
    yt += Ts*x;

    auto d = fabs(upper-lower);

    if( yt < lower ){
        yt+= d;
    }
   if( yt > upper ){
       yt -= d;
   }
    return yt;
   
}

//积分器，后向欧拉
static auto 积分( floatx x ,floatx Ts ,floatx y_n_1)
{   
    //y_n_1:上一次输出值
    auto yt = y_n_1;
    yt += Ts*x;  
    return yt;   
}

//低通滤波器，后向欧拉
static auto 滤波_低通( floatx x , floatx Ts ,floatx f , floatx y_n_1 )
{
    //y_n_1:上一次输出值，f:滤波频率
    auto T = 1/(2*π*f);
    auto TTs =  T + Ts;
    auto y = y_n_1 * T/TTs + x * Ts/TTs;
    return y ;
}

//PI调节器，积分采用后向欧拉,updata非零时复位   
static void PI计算( PI *pi ,floatx x ,int update , floatx Ts )
{
    if(update){
        //非零复位
        pi->y = pi->y_init;
        pi->yi = 0;
    }
    else{
        //不复位
        auto yp = pi->kp * x;
        auto yi = pi->yi + pi->ki * Ts * x ;//积分采用后向欧拉
        auto yt = yp +yi;

        //当模块组件之和超出输出限制且积分器输出与模块输入具有相同的符号时，积分停止;
        //当模块组件之和超出输出限制且积分器输出与模块输入具有相反的符号时，积分恢复;
        //钳位有时称为条件积分;
        //对于饱和时间相对较小的被控对象，钳位可能很有用；但对于饱和时间较大的被控对象，它可能产生不良的瞬时响应。
        auto 符号 = ( x>0&&yi>0 )||( x<0&&yi<0 );
        if (yt > pi->upper){
            yt = pi->upper;
            if(符号) yi = pi->yi;
        }
        if (yt < pi->lower) {
            yt = pi->lower;
            if(符号) yi = pi->yi;
        }
        pi->yi = yi;
        pi->y = yt;      
    }

 }


//SOGI正交信号生成，包含直流抑制
static void SOGI正交信号生成( SOGI *sogi ,floatx x ,floatx w0 , floatx Ts ){
    //SOGI 对象数据更新在函数最后

    auto ein  = x -( sogi->X0 + sogi->X1 );
    auto err0 = sogi->k0 *ein;    
    auto errk  = sogi->k  *ein - sogi->X2;

    auto X1 = 积分( w0*errk , Ts ,  sogi->X1 );
    auto X0 = 积分( w0*err0 , Ts ,  sogi->X0 );
    auto X2 = 积分( w0*X1   , Ts ,  sogi->X2 );

    sogi->X1 = X1;
    sogi->X2 = X2;
    sogi->X0 = X0;

}


// PLL_SOGI pll_Sogi = {
//     .θ = 0    ,
//     .f = 50   ,
//     .A = 0.01 ,
//     .f_lower = 36.16,    
//     .sPI1 = {
//         .kp = 94,
//         .ki = 3658,        
//         .upper = 2*π *50,
//         .lower = - 2*π *50,
//         .y_init = 0,
//         .yi = 0,
//         .y  = 0 
//     },
//     .PI_autoGain_min  = 0.01,    
//     .sSOGI1 = {
//         .k  =  1.45,
//         .k0 =  0.123,  
//         .X1 =  0,
//         .X2 =  0,
//         .X0 =  0
//     },       
// };

PLL_SOGI *PLL_SOGI_Creat()
{
    try
    {
        auto p = new PLL_SOGI{
            .θ = 0,
            .f = 50,
            .A = 0.01,
            .f_lower = 36.16,
            .sPI1 = {
                .kp = 94,
                .ki = 3658,
                .upper = 2 * π * 50,
                .lower = -2 * π * 50,
                .y_init = 0,
                .yi = 0,
                .y = 0},
            .PI_autoGain_min = 0.01,
            .sSOGI1 = {.k = 1.45, .k0 = 0.123, .X1 = 0, .X2 = 0, .X0 = 0},

        };
        return p;
    }
    catch (const std::bad_alloc &e)
    {
        return nullptr;
    }
    
}



/*    
    SOGI 锁相环
*/
void PLL_SOGI_Cal(
    PLL_SOGI *pll,
    float ui, float f0,
    float PI_kp, float PI_ki, float PI_upper, float PI_lower,
    float PI_autoGain_min,
    float f_lower,
    float SOG_k, float SOG_k0,
    float Ts,
    float *ptheta, float *pA, float *pf,
    int reset)
{
    
    auto 输入系数 = [=](PLL_SOGI *p) { 
        //外部变量都隐式按值捕获
        p->f_lower = f_lower;
        p->PI_autoGain_min = PI_autoGain_min;
        p->sSOGI1.k = SOG_k;
        p->sSOGI1.k0 = SOG_k0;
        p->sPI1.kp = PI_kp;
        p->sPI1.ki = PI_ki;
        p->sPI1.lower = PI_lower;
        p->sPI1.upper = PI_upper;
    };
    输入系数(pll);

    if (reset)
    { //非零复位

        //状态量复位
        pll->sSOGI1.X1 = 0;
        pll->sSOGI1.X2 = 0;
        pll->sSOGI1.X0 = 0;
        pll->中间量.q_err = 0;
        pll->sPI1.y = 0;
        pll->sPI1.yi = 0;
        pll->θ = 0;        
        //输出量
        pll->θ = 0;
        pll->f = 0;
        pll->A = 0;        
    }
    else
    { //不复位

        auto w = 2 * π * pll->f;
        SOGI正交信号生成(&pll->sSOGI1, ui, w, Ts);
        auto [d, q] = αβ2dq(pll->sSOGI1.X1, pll->sSOGI1.X2, pll->θ);
        auto A = fabs(d);                                                   //幅值
        auto Again = (A < pll->PI_autoGain_min) ? pll->PI_autoGain_min : A; //PI环自动增益系数
        pll->中间量.q_err = 滤波_低通(q, Ts, pll->f_lower, pll->中间量.q_err);
        PI计算(&pll->sPI1, pll->中间量.q_err / Again, 0, Ts);
        auto Δw = pll->sPI1.y;
        w = 2 * π * f0 + Δw;
        if (w < 0)
            w = 0;
        auto θ = 积分_循环(w, Ts, 0, 2 * π, pll->θ);

        pll->A = A;
        pll->f = w / (2 * π);
        pll->θ = θ;
    }

    auto 输出 = [=]( float *pptheta, float *ppA, float *ppf ) {
        //外部变量都隐式按值捕获 
        *pptheta = pll->θ;
        *ppA = pll->A;
        *ppf = pll->f;        
    };
    输出( ptheta , pA , pf );

    

}